#ifndef _IMU_SENSOR_H_
#define _IMU_SENSOR_H_

#include "stm32f10x.h"
#include "SpiPeripheral.h"
#include "LSM6DSL.h"

typedef enum {
    LSM6DSL_ACC_GYRO_ODR_XL_POWER_DOWN    =0x00,
    LSM6DSL_ACC_GYRO_ODR_XL_13Hz          =0x10,
    LSM6DSL_ACC_GYRO_ODR_XL_26Hz          =0x20,
    LSM6DSL_ACC_GYRO_ODR_XL_52Hz          =0x30,
    LSM6DSL_ACC_GYRO_ODR_XL_104Hz         =0x40,
    LSM6DSL_ACC_GYRO_ODR_XL_208Hz         =0x50,
    LSM6DSL_ACC_GYRO_ODR_XL_416Hz         =0x60,
    LSM6DSL_ACC_GYRO_ODR_XL_833Hz         =0x70,
    LSM6DSL_ACC_GYRO_ODR_XL_1660Hz        =0x80,
    LSM6DSL_ACC_GYRO_ODR_XL_3330Hz        =0x90,
    LSM6DSL_ACC_GYRO_ODR_XL_6660Hz        =0xA0,
} LSM6DSL_ACC_GYRO_ODR_XL_t;

typedef enum {
    LSM6DSL_ACC_GYRO_ODR_G_POWER_DOWN     =0x00,
    LSM6DSL_ACC_GYRO_ODR_G_13Hz           =0x10,
    LSM6DSL_ACC_GYRO_ODR_G_26Hz           =0x20,
    LSM6DSL_ACC_GYRO_ODR_G_52Hz           =0x30,
    LSM6DSL_ACC_GYRO_ODR_G_104Hz          =0x40,
    LSM6DSL_ACC_GYRO_ODR_G_208Hz          =0x50,
    LSM6DSL_ACC_GYRO_ODR_G_416Hz          =0x60,
    LSM6DSL_ACC_GYRO_ODR_G_833Hz          =0x70,
    LSM6DSL_ACC_GYRO_ODR_G_1660Hz         =0x80,
    LSM6DSL_ACC_GYRO_ODR_G_3330Hz         =0x90,
    LSM6DSL_ACC_GYRO_ODR_G_6660Hz         =0xA0,
} LSM6DSL_ACC_GYRO_ODR_G_t;

typedef enum
{
    LSM6DSL_STATUS_OK = 0,
    LSM6DSL_STATUS_ERROR,
    LSM6DSL_STATUS_TIMEOUT,
    LSM6DSL_STATUS_NOT_IMPLEMENTED
} LSM6DSLStatusTypeDef;

typedef enum {
    LSM6DSL_ACC_GYRO_FS_G_245dps          =0x00,
    LSM6DSL_ACC_GYRO_FS_G_500dps          =0x04,
    LSM6DSL_ACC_GYRO_FS_G_1000dps         =0x08,
    LSM6DSL_ACC_GYRO_FS_G_2000dps         =0x0C,
} LSM6DSL_ACC_GYRO_FS_G_t;

typedef enum {
    LSM6DSL_ACC_GYRO_FS_125_DISABLED          =0x00,
    LSM6DSL_ACC_GYRO_FS_125_ENABLED           =0x02,
} LSM6DSL_ACC_GYRO_FS_125_t;

typedef enum {
    LSM6DSL_ACC_GYRO_FS_XL_2g            =0x00,
    LSM6DSL_ACC_GYRO_FS_XL_16g           =0x04,
    LSM6DSL_ACC_GYRO_FS_XL_4g            =0x08,
    LSM6DSL_ACC_GYRO_FS_XL_8g            =0x0C,
} LSM6DSL_ACC_GYRO_FS_XL_t;

class ImuSensor
{
private:
    SpiPeripheral *_ptrans;
    uint8_t X_isEnabled;
    float X_Last_ODR;
    uint8_t G_isEnabled;
    float G_Last_ODR;
    float g_sensitivity;
    float x_sensitivity;

public:
    const static u8 device_id = 106; //device id always be 0x6A
    double angular_zero_rate; //静态角速度偏差

public:
    static ImuSensor* get(void);
    bool init(void);
    void init_static_drift(void);
    void writeRegister(u8 regaddr, u8 val);
    u8 readRegister(u8 regaddr);
    u8 getDeviceId(void);
    void refresh_zero_rate(void);

    /**
     * @brief Utility function to write data.
     * @param  pBuffer: pointer to data to be written.
     * @param  RegisterAddr: specifies internal address register to be written.
     * @param  NumByteToWrite: number of bytes to write.
     * @retval 0 if ok, an error code otherwise.
     */
    uint8_t IO_Write(uint8_t* pBuffer, uint8_t RegisterAddr, uint16_t NumByteToWrite)
    {
        u8 ret = 0;
        _ptrans->chipSelect();
        _ptrans->readAfterWrite(RegisterAddr&0x7F);
        for (int i = 0 ; i < NumByteToWrite ; i++)
            _ptrans->readAfterWrite(pBuffer[i]);
        _ptrans->chipRelease();
        return ret;
    }
    
    /**
     * @brief Utility function to read data.
     * @param  pBuffer: pointer to data to be read.
     * @param  RegisterAddr: specifies internal address register to be read.
     * @param  NumByteToRead: number of bytes to be read.
     * @retval 0 if ok, an error code otherwise.
     */
    uint8_t IO_Read(uint8_t* pBuffer, uint8_t RegisterAddr, uint16_t NumByteToRead)
    {
        _ptrans->chipSelect();
        _ptrans->readAfterWrite(RegisterAddr|0x80);

        for (int i = 0 ; i < NumByteToRead ; i++)
            pBuffer[i] = _ptrans->readAfterWrite(0xFF);

        _ptrans->chipRelease();
        return 0;
    }
    
    LSM6DSLStatusTypeDef Enable_X                     (void);
    LSM6DSLStatusTypeDef Enable_G                     (void);
    LSM6DSLStatusTypeDef Get_G_Axes                   (int32_t *pData);
    LSM6DSLStatusTypeDef Get_G_AxesRaw                (int16_t *pData);
    LSM6DSLStatusTypeDef Get_G_Sensitivity            (float *pfData);
    LSM6DSLStatusTypeDef Get_X_Axes                   (int32_t *pData);
    LSM6DSLStatusTypeDef Get_X_AxesRaw                (int16_t *pData);
    LSM6DSLStatusTypeDef Get_X_Sensitivity            (float *pfData);
    
private:
    ImuSensor(SpiPeripheral* trans);
    LSM6DSLStatusTypeDef Set_X_ODR_When_Enabled(float odr);
    LSM6DSLStatusTypeDef Set_G_ODR_When_Enabled(float odr);

};

#endif
